#if defined(PPM_OUTPUT)

static unsigned char ServoNumber = 0;
unsigned int total_ppm_time = 0;
boolean ppm_gap = false;
static int FIXED_GAP_uS = 792; // To match 40000 of Timer 1 (2 x 396) // PPM frame fixed LOW phase

// Faster than using the sluggish digitalWrite()
#define PPM_SHIFT_HIGH  PORTB |= _BV(0);   
#define PPM_SHIFT_LOW   PORTB &= ~_BV(0);

//int pulseMin = 1000; // pulse minimum width minus start in uS
//int pulseMax = 2000; // pulse maximum width in uS

//################# TIMER1 ##################
// Setting TIMER1
// Prescaler set to 1/8 CPU 16Mhz 
// Each cycle equals to 0.5uS giving a resolution of 2048 for each servo pulse
// and a 50Hz working frequency.
// Overflow set to 40000
void INIT_SERVO_DRIVER(void) {
  
   // Set PPMOUT pin as output
   pinMode(outPinPPM, OUTPUT);
   
   // Set TIMER1
   TCCR1B   =   0x00;  
   TCNT1H   =   0x00;  
   TCNT1L   =   0x00;
   ICR1     =   40000; 
   TCCR1A   =   0x02;   
   TCCR1B   =   0x1A;
   TIMSK1   =   _BV (TOIE1);   
} 

ISR(TIMER1_OVF_vect) {
  unsigned int us;
  
  // Set PPM port LOW
  PPM_SHIFT_LOW;
  
  // Check if we need to add the time gap between channels and then return
  if(ppm_gap) {
    ppm_gap = false;
    total_ppm_time += FIXED_GAP_uS;
    TCNT1 = 40000 - FIXED_GAP_uS;
    return;
  }

  ServoNumber++;
  
  if (ServoNumber>8) {
    total_ppm_time = 0;
    ServoNumber=0;
  }
 
  if (ServoNumber == 8) {
    us = 40000 - total_ppm_time;
  } else {
    us = (rxdata.AxisIn[ServoNumber]) - FIXED_GAP_uS;
  }
  
  ppm_gap = true;
  total_ppm_time += us;
  
  // Set PPM port HIGH
  PPM_SHIFT_HIGH; 
  
  // Set new timer goals
  TCNT1 = 40000 - us; 
}

#else

void writeServos() {
  for (byte i = 0; i < CHANNELS; i++ ){
    MyServos[i].writeMicroseconds(rxdata.AxisIn[i]/2);   
  }
}

#endif
